package TeleRobot;

import java.awt.Color;
import java.awt.Graphics;
import java.lang.Math;
import java.util.ArrayList;
import javax.swing.JFrame;

public class Robot2 {
    private Point a1;        //Axis 1 Location
    private Point a2;        //Axis 2 Location
    private Point a3;        //Axis 3 Location
    private Point pb;        //Paint Brush Location
    
    private double new_theta1;
    private double new_theta2;
    private double new_theta3;
    private double theta1;   //Axis 1 Angle
    private double theta2;   //Axis 2 Angle
    private double theta3;   //Axis 3 Angle
    private double phi = 0;  //Rotation about x axis for moving frame
    private double l1 = 150; //Length of Link 1
    private double l2 = 100; //Length of Link 2
    private double l3 = 75;  //Length of Link 3
    
    private boolean paint=false;
    
    private ArrayList<Arc> ovals = new ArrayList<Arc>();
    private ArrayList<Point> points = new ArrayList<Point>();
    
    Robot2()
    {
        
        //Initialize points -- correct points are recalculated before being drawn
        Point robot_base = new Point(200, 500);
        a1 = robot_base.clone();
        a2 = robot_base.clone();
        a3 = robot_base.clone();
        pb = robot_base.clone();
             
        //Initialize angles
        theta1 = 0;
        theta2 = 0;
        theta3 = 0;
        
        calc_points_from_angles();
        
    }
    
    Robot2(int len1, int len2, int len3, Point robot_base)
    {
        //Initialize link lengths
        l1 = len1;
        l2 = len2;
        l3 = len3;
        
        //Initialize points -- correct points are recalculated before being drawn
        a1 = robot_base.clone();
        a2 = robot_base.clone();
        a3 = robot_base.clone();
        pb = robot_base.clone();
        
        //Initialize angles
        theta1 = 0;
        theta2 = 0;
        theta3 = 0;
        
        calc_points_from_angles();
              
    }
    
    public void togglePaint()
    {
        paint=!paint;
        System.out.println("paint is now:" + paint);
    }
    
    public void draw(Graphics g)
    {        
        //calc_points_from_angles();
        
        g.setColor(Color.BLUE);
        g.drawLine((int)a1.x, (int)a1.y, (int)a2.x, (int)a2.y);
        g.setColor(Color.orange);
        g.drawLine((int)a2.x, (int)a2.y, (int)a3.x, (int)a3.y);
        g.setColor(Color.MAGENTA);
        g.drawLine((int)a3.x, (int)a3.y, (int)pb.x, (int)pb.y);
        g.setColor(Color.black);
        g.fillOval((int)a1.x-3, (int)a1.y-3,6,6);
        g.fillOval((int)a2.x-3, (int)a2.y-3,6,6);
        g.fillOval((int)a3.x-3, (int)a3.y-3,6,6);
        g.fillOval((int)pb.x-3, (int)pb.y-3,6,6);
        
        g.setColor(Color.red);
        for (int i=0;i<points.size();i++)
            g.fillOval((int)points.get(i).x-5,(int) points.get(i).y-5, 10, 10);
        
    }
    
    private void calc_points_from_angles()
    {
        //Calculates a1,a2,a3,pb from values of theta and lengths
//        System.out.println("Points recalculated based on:");
//        System.out.println("theta1: " + Math.toRadians(theta1));
//        System.out.println("theta2: " + Math.toRadians(theta2));
//        System.out.println("theta3: " + Math.toRadians(theta3));
        System.out.println("Recalculating...");
        a2.x = a1.x + (l1 * Math.cos(Math.toRadians(theta1)));
        a2.y = a1.y - (l1 * Math.sin(Math.toRadians(theta1)));
//        System.out.println("a1: " + a1);
//        System.out.println("a2: " + a2);
        
        a3.x = a2.x + (l2 * Math.cos(Math.toRadians(theta2 + theta1)));
        a3.y = a2.y - (l2 * Math.sin(Math.toRadians(theta2 + theta1)));
//        System.out.println("a3: " + a3);
        
        pb.x = a3.x + (l3 * Math.cos(Math.toRadians(theta3 + theta2 + theta1)));
        pb.y = a3.y - (l3 * Math.sin(Math.toRadians(theta3 + theta2 + theta1)));
//        System.out.println("pb: " + pb);
    }
    
    public Point getBrushLocation()
    {
        return pb;
    }
    
    public void moveBrush(String axis, int distance, Graphics g, JFrame canvasFrame) {
        if (axis.equals("x")) {
            System.out.println("Want to change x by: " + distance);
            //Check new pos is possible
            //Do something for x by distance
        }
        else {
            System.out.println("Want to change y by: " + distance);
            //Check new pos is possible
            //Do something for y by distance
        }
    }
    
    public void setParameters(int link, double offset) {
        String label;
        //System.out.println("Resetting parameters");
        //Recalculate angles
        if (link==1)
        {
            theta1+=offset;
            theta1=(theta1+360)%360;
            label = "THETA 1:";
            label+= String.valueOf((int)theta1);
            RoboticsApp.mainFrame.label_thetaone.setText(label);
        }
        else if(link==2)
        {
            theta2+=offset;
            theta2=(theta2+360)%360;
            label = "THETA 2:";
            label+= String.valueOf((int)theta2);
            RoboticsApp.mainFrame.label_thetatwo.setText(label);
        }
        else if(link==3)
        {
            theta3+=offset;
            theta3=(theta3+360)%360;
            label = "THETA 3:";
            label+= String.valueOf((int)theta3);
            RoboticsApp.mainFrame.label_thetathree.setText(label);
        }
        calc_points_from_angles();
    }
   
    public void redraw (Graphics g, int link, double offset)
    {
        //System.out.println("Redraw was called");
        setParameters(link,offset);
        
        if(paint)
        {
            System.out.println("Added a point at " + pb.x + "," + pb.y);
            points.add(new Point(pb.x,pb.y));
        }
    }
    
    public void drawExtraPoints(Point a, Point b)
    {
        System.out.println("Trying to add points from : " + a.x + "," + a.y + "to " + b.x + "," + b.y);
        Point start,end;
        if (a.x==b.x) {
            if(a.x<b.x) {
                start = new Point(a.x,a.y);
                end = new Point (b.x,b.y);
            }
            else {
                start = new Point (b.x,b.y);
                end = new Point (a.x,a.y);
            }
            while(true) {
                start.y+=3;
                points.add(new Point(start.x, start.y));
                if (start.y>=end.y) break;
            }
        }
        else {
            if(a.x<b.x) {
                start = new Point(a.x,a.y);
                end = new Point (b.x,b.y);
            }
            else {
                start = new Point (b.x,b.y);
                end = new Point (a.x,a.y);
            }
            double slope = ((double)(start.y-end.y))/((double)(start.x-end.x));
            System.out.println("slope is: " + slope);
            double x_change = Math.sqrt(25/(slope*slope+1));
            double y_change = Math.sqrt(25/(1+1/(slope*slope)));
            if (slope<0) y_change*=-1;
            while(true)
            {
                start.y+=y_change;
                start.x+=x_change;
                System.out.println("Adding Points at" + start.x + "," + start.y);
                if (start.x>=end.x) break;
                points.add(new Point(start.x, start.y));
            }
        }
    }
    
    public Boolean animateTwo(Point target_location)
    {
        double WristCenter_X = target_location.x - a1.x;
        double WristCenter_Y = target_location.y - a1.y;
        Point WristCenter = new Point(WristCenter_X, WristCenter_Y);
        double WristCenter_SquaredLength = WristCenter.length() * WristCenter.length();
          
        double OuterAngle = Math.atan2(WristCenter_Y, WristCenter_X);
        double IntermediateLength = ((l1*l1) + WristCenter_SquaredLength - (l2*l2))/(2*l1*WristCenter.length());
        //System.out.println("IntermediateLength:"+String.valueOf(IntermediateLength));
        double InnerAngle = Math.acos(IntermediateLength);
          
          //Check that solution exists
        if (IntermediateLength > 1.0)
        {
            return false;
        }
              
        new_theta1 = -1 * Math.toDegrees(InnerAngle + OuterAngle);
        double inner_elbowAngle = Math.acos(((l1*l1) + (l2*l2) - WristCenter_SquaredLength)/(2*l1*l2));
        new_theta2 = 180.0 - Math.toDegrees(inner_elbowAngle);
        return true;
    }
    
    public Boolean animateJoints(Graphics g, Point target_location)
    {
        double tdistance = (target_location.x-a1.x)*(target_location.x-a1.x)+(target_location.y-a1.y)*(target_location.y-a1.y);
        if (Math.sqrt(tdistance)>l1+l2+l3) {
            System.out.println("Too far away");
            return false;
        }
        Point old_pb = pb.clone();
        boolean worked=false;
        ArrayList<Point> circum = new ArrayList<Point>();
        double new_theta3;
        for (int i=0; i<1440; i++) {
            double circlex=target_location.x+l3*Math.cos(Math.toRadians(360-i));
            double circley=target_location.y+l3*Math.sin(Math.toRadians(360-i));
            worked=animateTwo(new Point(circlex,circley));
            if(worked) break;
        }
        double absCurrent = Math.sqrt(pb.x*pb.x + pb.y*pb.y);
        setParameters(1, (new_theta1 - theta1));
        setParameters(2, (new_theta2 - theta2));
        //System.out.println("a3: " + a3.x + "," + a3.y);
        double distx=target_location.x-a3.x;
        double disty=a3.y-target_location.y;
        double angle=0;
        double cosa=distx/l3;
        double sina=disty/l3;
        System.out.println("distx,disty: " +distx + " , " + disty);
        angle=Math.toDegrees(Math.atan2(sina,cosa));
        System.out.println("Angle is: " + angle);
        double dist=Math.sqrt(distx*distx+disty*disty);
        new_theta3=angle-theta1-theta2;   
        System.out.println("new_theta 1 is: " + new_theta1 + " new_theta 2 is: " + new_theta2  + " new_theta 3 is: " + new_theta3 );
        redraw(g, 3, (new_theta3 - theta3));
        double absTarget = Math.sqrt(pb.x*pb.x + pb.y*pb.y);
          
          double absDifference = Math.max(absTarget,absCurrent) - Math.min(absTarget,absCurrent);
          if (paint)
          {
              drawExtraPoints(old_pb, pb);
          }
          
          return true;
    }
    
    public Boolean animateJoints(Graphics g, Point target_location, JFrame canvasFrame)
    {
          boolean flag=animateJoints(g,target_location);
          if(flag) canvasFrame.repaint();
        
          return flag;
    }
    
    public Boolean NumericalAnimateJoints(Graphics g, Point target_location)
    {
        final double epsilon = 0.0001;
        double dist_pbx, dist_pby, dist_pb;
        double dist_targetx, dist_targety, dist_target;
        double cosRotAng, sinRotAng, endTargetMag;
        ArrayList<Point> joints = new ArrayList<Point>();
        
        //Add robot joints to list
        joints.add(a3);
        joints.add(a2);
        joints.add(a1);
        
        int joint_count = 1;
        for(Point joint: joints)
        {
           //Calculate distance from joint to the paintbrush
           dist_pbx = pb.x - joint.x;
           dist_pby = pb.y - joint.y;
           dist_pb = new Point(dist_pbx, dist_pby).length();
           
           //Calculate distance from joint to target position
           dist_targetx = target_location.x - joint.x;
           dist_targety = target_location.y - joint.y;
           dist_target = new Point(dist_targetx, dist_targety).length();
           
           //Get paintbrush rotation angle
           double err_target = dist_target*dist_target;
           if (err_target <= epsilon) //Threshold for no more rotations
           {
               cosRotAng = 1;
               sinRotAng = 0;
           }
           else
           {
               cosRotAng = ((dist_pbx*dist_targetx) + (dist_pby*dist_targety))/err_target;
               sinRotAng = ((dist_pbx*dist_targetx) - (dist_pby*dist_targety))/err_target;
           }
           
           //Angle to rotate joint
           double rotAng = Math.acos(Math.max(-1, Math.min(1, cosRotAng)));
           rotAng = Math.toDegrees(rotAng);
           redraw(g, joint_count, (int)rotAng);
           joint_count++;
        }
        return true;

    }
}
